#ifndef _FUNC_H_
#define _FUNC_H_

#include <iostream>
#include <string>
#include "all.h"
#include <qpOASES.hpp>

#include <webots/Robot.hpp>
#include <webots/Motor.hpp>
#include <webots/PositionSensor.hpp>
#include <webots/InertialUnit.hpp>
using namespace std;
using namespace Eigen;
using namespace webots;

void test_controller()
{

     /*-----------------------步态调度器测试-----------------------------------------------------------------
   //     float tStart = timetest();
     GaitPlanner gp;
     gp.setGait(2.0, 2.0, "trot");
     gp.calculateGait(3.14);

     for (int i = 0; i < 4; i++)
     {
         cout << "是否处于支撑相：" << gp.s_fai_[i] << "\t";
         cout << "支撑相相位进度：" << gp.t_st_norm_[i] << "\t";
         cout << "摆动相相位进度：" << gp.t_sw_norm_[i] << "\t";
         cout << endl;
     }
         // Sleep(1000);
      //    float tEnd = timetest();
      //    cout<<tEnd-tStart;
     -----------------------------------------------------------------------------------------------------*/

         /*-----------------------状态估计器测试----------------------------------------------------------------
     //     float tStart = timetest();
     // for (int iii = 0; iii < 1000; iii++)
     // {
     StateEstimator se;
     Eigen::Matrix3d R_imu_imu0;

     R_imu_imu0 << 0.1, 0.2, 0.3,
         0.4, 0.5, 0.6,
         0.7, 0.8, 0.9;

     cout << R_imu_imu0 << endl;

     Matrix<double, 12, 1> q_j_i;
     q_j_i << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 0.10,
         0.11, 0.12;
     Matrix<double, 12, 1> q_dot_j_i;
     q_dot_j_i << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 0.10,
         0.11, 0.12;
     Vector3d a_original;
     a_original << 1, 2, 3;
     Vector3d omega_original;
     omega_original << 1, 2, 3;

     se.setIMUdata(R_imu_imu0, a_original, omega_original);
     se.setEncoderdata(q_j_i, q_dot_j_i);

     se.calculateAll();
     cout << "------------x_k------------" << endl
          << se.getx_k() << endl;
     // }
     //     float tEnd = timetest();
     //     cout << tEnd - tStart;
         ------------------------------------------------------------------------------------------------*/

     /*-----------------------qpOASES测试-------------------------------------------------------------------

         // 1. 构建QP问题
     QProblem example(2, 1); // 优化变量, 约束变量 维度

     // 2. 初始化第一个QP
     real_t H[2*2] = {1.0, 0.0, 0.0, 0.5}; // Hessian Matrix
     real_t A[1*2] = {1.0, 1.0};                    // 不等式/等式约束系数矩阵
     real_t g[2] = {1.5, 1.0};                      // 线性项系数 与maatlab不同为 x' * g
     real_t lb[2] = {0.5, -2.0};                    // x 下界
     real_t ub[2] = {5.0, 2.0};                     // x 上界
     real_t lbA[1] = {-1.0};                             // 不等式/等式上界 上下界相等时为等式约束
     real_t ubA[1] = {2.0};                              // 不等式/等式下界

     int nWSR = 10;

     example.init(H, g, A, lb, ub, lbA, ubA, nWSR); // 求解

     // 4. 得到最优解
     real_t xOpt[2];
     real_t yOpt[2+1];
     example.getPrimalSolution(xOpt);
     example.getDualSolution(yOpt);
     std::cout<<"PrimalSolution: "<<xOpt[0]<<","<<xOpt[1]<<std::endl; // 原问题最优变量
     std::cout<<"DualSolution: "<<yOpt[0]<<","<<yOpt[1]<<yOpt[2]<<","<<std::endl; // 最优对偶变量(优化变量个数+约束个数)
     std::cout<<"ObjVal: "<<example.getObjVal()<<std::endl; // 目标函数值
     ------------------------------------------------------------------------------------------------------*/

     // // MPCsolver ms;
     // // GaitPlanner gp;
     // // StateEstimator se;
     // TrajectoryGenerator tg;

     // // gp.setGait(2, 2, "trot");
     // // gp.calculateGait(3.14);

     // tg.setExpection(50, 20, 10);
     // Matrix3d RBO = R("x", 70)*R("y", 50);

     // tg.calculateAll(&RBO);

     // Matrix<double, 13*PREDICTION_HORIZON, 1> vecD;
     // tg.getD(vecD);

     // cout<<vecD<<endl;
     // cout<<vecD.size();
}

void spot_IMUtest()
{
     // spot系与世界系重合时，欧拉角均为零
     cout << "IMUtest" << endl;
     Robot *robot = new Robot();
     int time_step = robot->getBasicTimeStep();

     InertialUnit *imu = robot->getInertialUnit("inertial unit");
     imu->enable(time_step);

     while (robot->step(2*time_step) != -1)
     {
          const double *rpy = imu->getRollPitchYaw();
          cout << "roll: " << rpy[0] << endl;
          cout << "pitch: " << rpy[1] << endl;
          cout << "yaw: " << rpy[2] << endl;
     }

     delete robot;
}



int mainmain()
{
     // test_main();
     return 0;
}

#endif